import numpy as np
import ctypes as ct

class JointID(object):
	headJoints = range(4);
	torsoJoints = range(4,8)
	leftArm = range(8,16);
	rightArm = range(16,24);
	leftLeg = range(24,31);
	rightLeg = range(31,38)
	leftHand = range(38,41);
	rightHand = range(41,44)

class JointIO(object):
	def __init__(self):
		self.cjio = ct.CDLL('bin\JointIO.dll')
		self.ptr = ct.c_void_p();
		self.cjio.JointIO_create(ct.byref(self.ptr));
		self.numJoint = self.cjio.JointIO_getNumJoint(self.ptr);
		self.nExtra=2;
		print "numJoint:",self.numJoint

	def read(self):
		doubleArray= ct.c_double * (3*self.numJoint-6+self.nExtra); #each joint (2pressure + 1pot)  - no-pot on finger joins (last6)+ extra(force,pressure)
		data=doubleArray();
		self.cjio.JointIO_read(self.ptr, data);
		return data

	def readAll2(self):
		doubleArray= ct.c_double * self.numJoint;
		pot=doubleArray();
		psA=doubleArray();
		psB=doubleArray();
		extra=doubleArray();
		self.cjio.JointIO_readJoints(self.ptr, pot, psA, psB, extra, self.numJoint);
		return pot,psA,psB

	def readAll(self):
		pot=np.ndarray(self.numJoint);
		psA=np.ndarray(self.numJoint);
		psB=np.ndarray(self.numJoint);
		extra=np.ndarray(self.nExtra);
		self.cjio.JointIO_readJoints(self.ptr, 
			pot.ctypes.data_as(ct.POINTER(ct.c_double)), 
			psA.ctypes.data_as(ct.POINTER(ct.c_double)), 
			psB.ctypes.data_as(ct.POINTER(ct.c_double)), 
			extra.ctypes.data_as(ct.POINTER(ct.c_double)), 
			self.numJoint);
		return pot,psA,psB

	def write(self, data):
		self.cjio.JointIO_writeValves(self.ptr, 
						data.ctypes.data_as(ct.POINTER(ct.c_double)),
						len(data));

	def __del__(self):
		self.cjio.JointIO_destroy(self.ptr);


if __name__ == '__main__':
	J = JointIO();
	from timeit import Timer
	#print Timer('J.read2()','from __main__ import J').timeit(1000); #comparable performance
	print "1K 1khz blocking reads (sec) :", Timer('J.read()','from __main__ import J').timeit(1000);
	print "1K 1khz blocking readAlls (sec) :", Timer('J.readAll()','from __main__ import J').timeit(1000);
	print "1k nonblocking writes (sec) :", Timer('J.write(3.5*np.ones(2*J.numJoint))','from __main__ import J,np').timeit(1000);
